#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>

#include "sample_comm_nnie.h"
#include "sample_media_ai.h"
#include "ai_infer_process.h"
#include "vgs_img.h"
#include "ive_img.h"
#include "misc_util.h"
#include "hisignalling.h"

#include "face_detect.h"
#include "face_align.h"
#include "face_recognize.h"

#include <dirent.h>
#include <unistd.h>
#include "jpg2yuv_opencv.h"

#ifdef __cplusplus
#if __cplusplus
extern "C" {
#endif
#endif /* End of #ifdef __cplusplus */

#define DETECT_OBJ_MAX     32
#define RET_NUM_MAX        4
#define DRAW_RETC_THICK    8    // Draw the width of the line
#define WIDTH_LIMIT        32
#define HEIGHT_LIMIT       32
#define IMAGE_WIDTH        112  // The resolution of the model IMAGE sent to the classification is 224*224
#define IMAGE_HEIGHT       112
#define MODEL_FILE_FACE    "/userdata/models/face_detect/mnet_inst.wk" // darknet framework wk model

#define FACE_FRM_WIDTH    640
#define FACE_FRM_HEIGHT   640
#define DRAW_LDS_THICK    8 

#define FACE_DIS_TOLERANCE  11

#define TXT_BEGX            20
#define TXT_BEGY            20

static int biggestBoxIndex;
static IVE_IMAGE_S img;
static RectBox boxs[DETECT_OBJ_MAX] = {0};
static RectBox faceBox_max[DETECT_OBJ_MAX] = {0};
static RectBox remainingBoxs[DETECT_OBJ_MAX] = {0};
static RectBox recBoxs[DETECT_OBJ_MAX] = {0}; // Store the results of the classification network
static LandMark recPts[DETECT_OBJ_MAX] = {0};
static RecogNumInfo numInfo[RET_NUM_MAX] = {0};
static IVE_IMAGE_S imgIn;
static IVE_IMAGE_S imgDst;

/**************osd******************/
static OsdSet* g_osdsFace = NULL;
static HI_S32 g_osd0Face = -1;


static DetectFaceInfo faces[DETECT_OBJ_MAX] = {0};
static LandMark ptss[DETECT_OBJ_MAX] = {0};
 
float features[FETIRES_LEN] = {0};
//float features_from_img[80][FETIRES_LEN];   //用于保存人脸数据库的推理数据
static FaceFileInfo FaceFile[200] = {0};

HI_U32 font_size = 80;
int Face_Get_Img(char *dirname)
{
	//1, 打开目录
	DIR *dp = opendir(dirname);
	if(NULL == dp)
	{
		perror("opendir failed");
		return -1;
	}
	
	//2, 循环读取目录下的文件信息 --> 直到读取完毕
	int i = 0;
	struct dirent *fp;
	char filepath[PATH_MAX];			//存放文件的完整路径
	while((fp = readdir(dp)))	//循环读取目录中的信息,如果返回值为NULL,结束循环
	{
		if( strcmp(".", fp->d_name) == 0 ||  strcmp("..", fp->d_name) == 0  )
		{
			continue;
		}
		//筛选出.jpg结尾的文件 --> 判断检索的文件名最后4个字符是不是 ".jpg"
		if( strcmp(".jpg", fp->d_name+strlen(fp->d_name)-4) == 0 )
		{
			//读取的文件名和目录名进行拼接
			sprintf(filepath, "%s/%s", dirname, fp->d_name);
			strcpy( FaceFile[i].imgpath ,filepath);		//检索的文件的路径保存在数组PicPath中
        //    GetPicWidthHeight(FaceFile[i].imgpath,&(FaceFile[i].imgWidth),&(FaceFile[i].imgHeight));
			printf("filname[%d]:%s\n", i,FaceFile[i].imgpath);
        //    printf(" siez w = %d ,h = %d \n",FaceFile[i].imgWidth,FaceFile[i].imgHeight);
            i++;
		}
		
	}
	printf("Total of %d face data files!\n", i);
	//3, 关闭目录
	closedir(dp);	
	return i;
}

char *  face_path =  "//userdata/data/img_file/face"; 
//人脸数据数量
HI_S32 FaceFileNum = 0;
HI_S32 FaceFileCnt = 0;
HI_S32 Img_Input_Flag = 0;
int uartFd1 = 0;
HI_S32 RetinaFaceDetectLoad(uintptr_t* model, OsdSet* osds)
{
    SAMPLE_SVP_NNIE_CFG_S *self = NULL;
    HI_S32 ret;

    ret = OsdLibInit();
    HI_ASSERT(ret == HI_SUCCESS);

    g_osdsFace = osds;                                  //显示文字用
    HI_ASSERT(g_osdsFace);
    g_osd0Face = OsdsCreateRgn(g_osdsFace);
    HI_ASSERT(g_osd0Face >= 0);

    ret = RetinaFaceCreate(&self, MODEL_FILE_FACE);
    *model = ret < 0 ? 0 : (uintptr_t)self;

    FaceRecInit(); // Initialize the face recognization model
    SAMPLE_PRT("Load face detect model success\n");

    /* uart open init */
    uartFd1 = UartOpenInit();
    if (uartFd1 < 0) {
        printf("uart1 open failed\r\n");
    } else {
        printf("uart1 open successed\r\n");
    }
    return ret;
}

HI_S32 RetinaFaceDetectUnLoad(uintptr_t model)
{
    CnnFaceDetectDestroy((SAMPLE_SVP_NNIE_CFG_S*)model);
    FaceRecExit(); // Uninitialize the face recognization model
    SAMPLE_PRT("Unload Face Detect model success\n");
    OsdsClear(g_osdsFace);

    return 0;
}
/* Get the maximum face */
static HI_S32 GetBiggestFaceIndex(RectBox boxs[], int detectNum)
{
    HI_S32 faceIndex = 0;
    HI_S32 biggestBoxIndex = faceIndex;
    HI_S32 biggestBoxWidth = boxs[faceIndex].xmax - boxs[faceIndex].xmin + 1;
    HI_S32 biggestBoxHeight = boxs[faceIndex].ymax - boxs[faceIndex].ymin + 1;
    HI_S32 biggestBoxArea = biggestBoxWidth * biggestBoxHeight;

    for (faceIndex = 1; faceIndex < detectNum; faceIndex++) {
        HI_S32 boxWidth = boxs[faceIndex].xmax - boxs[faceIndex].xmin + 1;
        HI_S32 boxHeight = boxs[faceIndex].ymax - boxs[faceIndex].ymin + 1;
        HI_S32 boxArea = boxWidth * boxHeight;
        if (biggestBoxArea < boxArea) {
            biggestBoxArea = boxArea;
            biggestBoxIndex = faceIndex;
        }
        biggestBoxWidth = boxs[biggestBoxIndex].xmax - boxs[biggestBoxIndex].xmin + 1;
        biggestBoxHeight = boxs[biggestBoxIndex].ymax - boxs[biggestBoxIndex].ymin + 1;
    }

    if ((biggestBoxWidth == 1) || (biggestBoxHeight == 1) || (detectNum == 0)) {
        biggestBoxIndex = -1;
    }

    return biggestBoxIndex;
}
float   Dis = 999;  //欧氏距离
float   DisMin = 999;  
HI_S32  DisMinIndex = -1;
HI_S32  LOADED_FLAG = 0;
char    uart_buf[20];
struct timespec start_tim,end_tim;
long    uart_send_time;
HI_S32 RetinaFaceDetectCal(uintptr_t model, VIDEO_FRAME_INFO_S *srcFrm, VIDEO_FRAME_INFO_S *dstFrm)
{
    SAMPLE_SVP_NNIE_CFG_S *self = (SAMPLE_SVP_NNIE_CFG_S*)model; // reference to SDK sample_comm_nnie.h Line 99
    IVE_IMAGE_S img; // referece to SDK hi_comm_ive.h Line 143

    HI_OSD_ATTR_S rgn;

    int FaceNum;
    int num = 0;

    FILE *pFpSrc; 

    static HI_CHAR prevOsd[NORM_BUF_SIZE] = "";
    HI_CHAR osdBuf[NORM_BUF_SIZE] = "";

    HI_S32 ret;
    if(!LOADED_FLAG)                                  //加载脸部数据标志
    {
        strcpy(osdBuf,"Loading...");
        HiStrxfrm(prevOsd, osdBuf, sizeof(prevOsd));  //在屏幕显示
        // Superimpose graphics into resFrm
        TxtRgnInit(&rgn, osdBuf, 460, 490, ARGB1555_WHITE,100); // font width and heigt use default 40
        OsdsSetRgn(g_osdsFace, g_osd0Face, &rgn);
        printf("Recognition results:%s\n",osdBuf);
        ret = HI_MPI_VPSS_SendFrame(0, 0, dstFrm, 0);     
        if (ret != HI_SUCCESS) {
            SAMPLE_PRT("Error(%#x), HI_MPI_VPSS_SendFrame failed!\n", ret);
        }

        memset_s(FaceFile, sizeof(*FaceFile), 0x00, sizeof(*FaceFile));         //初始化都设为0
        FaceFileNum = Face_Get_Img(face_path);                                  //读取人脸数据库图片
        /**********更新数据库再转**********/
        for(int i = 0; i < FaceFileNum; i++)
        {    
            jpg2yuv420 (FaceFile[i].imgpath,FaceFile[i].yuvpath,&(FaceFile[i].imgWidth),&(FaceFile[i].imgHeight));
            printf("num = %d ,img path = %s ,yuv path = %s ,w = %d ,h = %d\n",i, FaceFile[i].imgpath,FaceFile[i].yuvpath,FaceFile[i].imgWidth,FaceFile[i].imgHeight);
        }
        /**********更新数据库再转**********/
        Img_Input_Flag = (FaceFileNum > 0 ? 1 :0);
        LOADED_FLAG = 1;
    }


    if(Img_Input_Flag)                                      //载入人脸数据库,用于输出人脸数据库里的比对数据
    {   
        printf("FaceFileCnt = %d ,FaceFileNum = %d\n",FaceFileCnt,FaceFileNum); 
        printf("%s \n",FaceFile[FaceFileCnt].yuvpath); 
        IveImgCreate(&img, IVE_IMAGE_TYPE_YUV420SP, FaceFile[FaceFileCnt].imgWidth,FaceFile[FaceFileCnt].imgHeight);
        printf("FaceFile[FaceFileCnt].imgWidth = %d,FaceFile[FaceFileCnt].imgHeight = %d\n",FaceFile[FaceFileCnt].imgWidth,FaceFile[FaceFileCnt].imgHeight); 
        pFpSrc = fopen(FaceFile[FaceFileCnt].yuvpath, "rb");
        HI_S32 s32Ret = SAMPLE_COMM_IVE_ReadFile(&img, pFpSrc); //从ImgPath(路径)读出图片数据
        SAMPLE_CHECK_EXPR_RET(HI_SUCCESS != s32Ret, s32Ret, "Error(%#x), Read src file failed!\n", s32Ret);
        fclose(pFpSrc);     
    }
    else
    {   
        ret = FrmToOrigImg((VIDEO_FRAME_INFO_S*)srcFrm, &img);
        SAMPLE_CHECK_EXPR_RET(ret != HI_SUCCESS, ret, "CnnFaceDetectCal FAIL, for YUV2RGB FAIL, ret=%#x\n", ret);
    }

    ret = RetinaFaceCalImg(self, &img,faces,DETECT_OBJ_MAX,&FaceNum); 
    SAMPLE_CHECK_EXPR_RET(ret < 0, "cnn cal FAIL, ret=%d\n", ret);
    printf("FaceNum:%d\n", FaceNum);
    /*
        处理结果
    */
    if(FaceNum)
    {     
        for (int i = 0; i < FaceNum; i++)
        {   
            recPts[i] = faces[i].pts;                           //用于识别
            recBoxs[i] = faces[i].box;
            RectBox *box = &faces[i].box;
            LandMark *pts = &faces[i].pts;
            RectBoxTran(box, FACE_FRM_WIDTH, FACE_FRM_HEIGHT,           //box相对坐标进行转换，并保存到boxs
                dstFrm->stVFrame.u32Width, dstFrm->stVFrame.u32Height);
            boxs[i] = *box;     
            LandMarksTran(pts, FACE_FRM_WIDTH, FACE_FRM_HEIGHT,           //box相对坐标进行转换，并保存到boxs
                dstFrm->stVFrame.u32Width, dstFrm->stVFrame.u32Height); 
            ptss[i] = *pts;    
        }
        biggestBoxIndex = GetBiggestFaceIndex(boxs, FaceNum);        //获取最大的目标框
        //SAMPLE_PRT("biggestBoxIndex:%d, FaceNum:%d\n", biggestBoxIndex, FaceNum);
        faceBox_max[0] = boxs[biggestBoxIndex];
        if(!Img_Input_Flag)                                      //载入人脸数据库,用于输出人脸数据库里的比对数据
        { 
            MppFrmDrawRects(dstFrm, faceBox_max, 1, RGB888_GREEN, DRAW_RETC_THICK); // Target hand objnum is equal to 1
        
            for (int j = 0; (j < FaceNum) && (FaceNum > 1); j++) {
                if (j != biggestBoxIndex) {
                    remainingBoxs[num++] = boxs[j];
                    // others hand objnum is equal to objnum -1
                    MppFrmDrawRects(dstFrm, remainingBoxs, FaceNum - 1, RGB888_RED, DRAW_RETC_THICK);
                }
            }
            //MppFrmDrawLandmarks(dstFrm, ptss, boxs ,FaceNum, DRAW_LDS_THICK);       //绘制5个人脸关键点
        }
        // Crop the image to classification network
        if(faceBox_max[0].ymax < 1070 && faceBox_max[0].ymin >= 0
        && faceBox_max[0].xmax < AICSTART_VI_OUTWIDTH  && faceBox_max[0].xmin >= 0)      //避免越界，程序跑了
        {
            //printf("faceBox_max1 :xmin = %d ,xmax = %d , ymin =%d ,ymax = %d\n",faceBox_max[0].xmin,faceBox_max[0].xmax,
            //faceBox_max[0].ymin,faceBox_max[0].ymax);
            ret = ImgYuvCrop(&img, &imgIn, &recBoxs[biggestBoxIndex]);              //根据box坐标裁剪最大人脸图像
            SAMPLE_CHECK_EXPR_RET(ret < 0, ret, "ImgYuvCrop FAIL, ret=%#x\n", ret);
            recPts[biggestBoxIndex].Eye_L.x = recPts[biggestBoxIndex].Eye_L.x - recBoxs[biggestBoxIndex].xmin;
            recPts[biggestBoxIndex].Eye_L.y = recPts[biggestBoxIndex].Eye_L.y - recBoxs[biggestBoxIndex].ymin;
            recPts[biggestBoxIndex].Eye_R.x = recPts[biggestBoxIndex].Eye_R.x - recBoxs[biggestBoxIndex].xmin;
            recPts[biggestBoxIndex].Eye_R.y = recPts[biggestBoxIndex].Eye_R.y - recBoxs[biggestBoxIndex].ymin;
            recPts[biggestBoxIndex].Nose.x  = recPts[biggestBoxIndex].Nose.x - recBoxs[biggestBoxIndex].xmin;
            recPts[biggestBoxIndex].Nose.y  = recPts[biggestBoxIndex].Nose.y - recBoxs[biggestBoxIndex].ymin;
            recPts[biggestBoxIndex].Mouth_L.x = recPts[biggestBoxIndex].Mouth_L.x - recBoxs[biggestBoxIndex].xmin;
            recPts[biggestBoxIndex].Mouth_L.y = recPts[biggestBoxIndex].Mouth_L.y - recBoxs[biggestBoxIndex].ymin;
            recPts[biggestBoxIndex].Mouth_R.x = recPts[biggestBoxIndex].Mouth_R.x - recBoxs[biggestBoxIndex].xmin;
            recPts[biggestBoxIndex].Mouth_R.y = recPts[biggestBoxIndex].Mouth_R.y - recBoxs[biggestBoxIndex].ymin;

        /*********保存文件图像*************
                FILE *pFpSrc;
                FILE *pFpDst;
                HI_CHAR achDstFileName[PATH_MAX] = {0};
                HI_CHAR achSrcFileName[PATH_MAX] = {0};
        *II*******保存文件图像***************/
                if ((imgIn.u32Width >= WIDTH_LIMIT) && (imgIn.u32Height >= HEIGHT_LIMIT)) {     //筛选 太小不要
                    
                    /******人脸对齐********/
                    //创建目标图像（分配内存）
                    SAMPLE_COMM_IVE_CreateImage(&imgDst, IVE_IMAGE_TYPE_YUV420SP, IMAGE_WIDTH, IMAGE_WIDTH);
                    FACE_ALIGN_IVE_PerspTrans(&imgIn, &imgDst,IMAGE_WIDTH,IMAGE_HEIGHT, &recPts[biggestBoxIndex]);
        /********保存文件图像**************
                     printf("imgIn.w = %d ,imgIn.h = %d\n",imgIn.u32Width, imgIn.u32Height);
                    printf("imgDst.w = %d ,imgDst.h = %d\n",imgDst.u32Width, imgDst.u32Height);

                    if (snprintf_s(achSrcFileName, sizeof(achSrcFileName), sizeof(achSrcFileName) - 1,
                        "./data/output/psp/complete_%s.yuv", "imgIn") < 0) {
                        HI_ASSERT(0);
                        }
                    printf("Src.h = %d, Src.w = %d\n", imgIn.u32Height,imgIn.u32Width);
                    pFpSrc = fopen(achSrcFileName, "wb");
                    SAMPLE_COMM_IVE_WriteFile(&imgIn, pFpSrc); // write yuv file

                    if (snprintf_s(achDstFileName, sizeof(achDstFileName), sizeof(achDstFileName) - 1,
                        "./data/output/psp/complete_%s.yuv", "imgDst") < 0) {
                        HI_ASSERT(0);
                        }
                    printf("Dst.h = %d, Dst.w = %d\n", imgDst.u32Height,imgDst.u32Width);
                    pFpDst = fopen(achDstFileName, "wb");
                    SAMPLE_COMM_IVE_WriteFile(&imgDst, pFpDst); // write yuv file
                    IVE_CLOSE_FILE(pFpSrc);
                    IVE_CLOSE_FILE(pFpDst);
        *******保存文件图像***************/
                    //memset_s(features, sizeof(*features), 0x00,sizeof(*features));
                    FaceRecCal(&imgDst, &features);
                    char *name_beg;
                    char *name_end;
                    char *id;
                    if(Img_Input_Flag)             
                    {   
                        FaceFile[FaceFileCnt].Face_Exist_FLAG = true;       //图片存在人脸
                        id       = strrchr(FaceFile[FaceFileCnt].yuvpath,'/') + 1;
                        name_beg = strrchr(FaceFile[FaceFileCnt].yuvpath,'/') + 2;
                        name_end = strrchr(FaceFile[FaceFileCnt].yuvpath,'_');
                        if(name_beg && name_end)
                        {   
                            if(name_end > name_beg)
                            {   
                                FaceFile[FaceFileCnt].id = *id - 0x30;
                                strncpy(FaceFile[FaceFileCnt].who,name_beg + 1,name_end-name_beg-1);
                                FaceFile[FaceFileCnt].who[name_end-name_beg-1] ='\0';
                                //printf("name = %s,id = %d\n",FaceFile[FaceFileCnt].who,FaceFile[FaceFileCnt].id);
                            }
                        }
                        else
                            strcpy(FaceFile[FaceFileCnt].who ,"Unknown");		
                        //printf("%d face file result:"); 
                        for(int i=0; i < FETIRES_LEN; i++)
                        {
                            FaceFile[FaceFileCnt].features[i] = features[i];  
                            //printf("%f ",features[i];              
                        }
                    }
                    else
                    {

                        for(int i=0; i < FaceFileNum;i++)
                        {   
                            if(FaceFile[i].Face_Exist_FLAG)                 //筛选图片数据有脸部的图片
                            {
                                FaceRecCompare(features,FaceFile[i].features, FACE_DIS_TOLERANCE, &Dis);  //实时视频帧与图片数据对比
                                if(DisMin >= Dis)                           //筛选脸部最小欧氏距离
                                {    
                                    DisMin = Dis;
                                    Dis = 999;
                                    DisMinIndex = i;                        
                                }
                            }
                        // printf("face_DisMin = %lf\n",DisMin);
                        }
                        if(DisMin <= FACE_DIS_TOLERANCE && DisMinIndex)
                        { 
                            clock_gettime(0, &(end_tim));
                            uart_send_time = end_tim.tv_sec - start_tim.tv_sec;
                            
                            strcpy(osdBuf,FaceFile[DisMinIndex].who);
                            if(uart_send_time > 3)
                            {
                                uart_buf[0] = 0xAA; //帧头
                                uart_buf[1] = 0xCC; 
                                uart_buf[2] = FaceFile[DisMinIndex].id;
                                uart_buf[3] = 0xCC;        
                                uart_buf[4] = 0xAA;                             //帧尾
                                UartSend(uartFd1, uart_buf, 5);
                                clock_gettime(0, &(start_tim));
                                uart_send_time=0;
                            }
                        }
                        else
                            strcpy(osdBuf,"Unknown");
                        DisMinIndex = -1;
                        DisMin = 999;
                    }
                    IveImgDestroy(&imgDst);
                }
            IveImgDestroy(&imgIn);
        }
        else
        {
            strcpy(osdBuf,"Close to the Center");
        }
    }
    else
    {
        strcpy(osdBuf,"No Face");
    }

    if (strcmp(osdBuf, prevOsd) != 0) {   
                HiStrxfrm(prevOsd, osdBuf, sizeof(prevOsd));  //在屏幕显示
                // Superimpose graphics into resFrm
                TxtRgnInit(&rgn, osdBuf, TXT_BEGX, TXT_BEGY, ARGB1555_YELLOW2,font_size); // font width and heigt use default 40
                OsdsSetRgn(g_osdsFace, g_osd0Face, &rgn);
                printf("Recognition results:%s\n",osdBuf);
                ret = HI_MPI_VPSS_SendFrame(0, 0, dstFrm, 0);     
                if (ret != HI_SUCCESS) {
                    SAMPLE_PRT("Error(%#x), HI_MPI_VPSS_SendFrame failed!\n", ret);
                }
        }

    if(FaceFileCnt==(FaceFileNum-1)) 
        Img_Input_Flag = 0; 
    else
        FaceFileCnt++; 
    //printf("Img_Input_Flag = %d , FaceFileCnt = %d\n",Img_Input_Flag,FaceFileCnt);   
    return ret;
}

#ifdef __cplusplus
#if __cplusplus
}
#endif
#endif /* End of #ifdef __cplusplus */
